#pragma once
#include <iostream>
#include <vector>
#include <deque>
#include <memory>
#include <array>
#include<cmath>
#include "kcf/kcftracker.hpp"
#include "defines.h"
#include "Kalman.h"
#include "common.h"
// --------------------------------------------------------------------------
///
/// \brief The TrajectoryPoint struct
///
struct TrajectoryPoint
{
	///
	/// \brief TrajectoryPoint
	///
	TrajectoryPoint()
		: m_hasRaw(false)
	{
	}

	///
	/// \brief TrajectoryPoint
	/// \param prediction
	///
	TrajectoryPoint(const Point_t& prediction)
		:
		m_hasRaw(false),
		m_prediction(prediction)
	{
	}

	///
	/// \brief TrajectoryPoint
	/// \param prediction
	/// \param raw
	///
	TrajectoryPoint(const Point_t& prediction, const Point_t& raw)
		:
		m_hasRaw(true),
		m_prediction(prediction),
		m_raw(raw)
	{
	}

	bool m_hasRaw;
	Point_t m_prediction;
	Point_t m_raw;
};

// --------------------------------------------------------------------------
///
/// \brief The Trace class
///
class Trace
{
public:
	///
	/// \brief operator []
	/// \param i
	/// \return
	///
	const Point_t& operator[](size_t i) const
	{
		return m_trace[i].m_prediction;
	}

	///
	/// \brief operator []
	/// \param i
	/// \return
	///
	Point_t& operator[](size_t i)
	{
		return m_trace[i].m_prediction;
	}

	///
	/// \brief at
	/// \param i
	/// \return
	///
	const TrajectoryPoint& at(size_t i) const
	{
		return m_trace[i];
	}

	///
	/// \brief size
	/// \return
	///
	size_t size() const
	{
		return m_trace.size();
	}

	///
	/// \brief push_back
	/// \param prediction
	///
	void push_back(const Point_t& prediction)
	{
		m_trace.push_back(TrajectoryPoint(prediction));
	}
	void push_back(const Point_t& prediction, const Point_t& raw)
	{
		m_trace.push_back(TrajectoryPoint(prediction, raw));
	}

	///
	/// \brief pop_front
	/// \param count
	///
	void pop_front(size_t count)
	{
		if (count < size())
		{
			m_trace.erase(m_trace.begin(), m_trace.begin() + count);
		}
		else
		{
			m_trace.clear();
		}
	}

	///
	/// \brief GetRawCount
	/// \param lastPeriod
	/// \return
	///
	size_t GetRawCount(size_t lastPeriod) const
	{
		size_t res = 0;

		size_t i = 0;
		if (lastPeriod < m_trace.size())
		{
			i = m_trace.size() - lastPeriod;
		}
		for (; i < m_trace.size(); ++i)
		{
			if (m_trace[i].m_hasRaw)
			{
				++res;
			}
		}

		return res;
	}

	const TrajectoryPoint end() const {
		return m_trace.back();
	}

private:
	std::deque<TrajectoryPoint> m_trace;
};

// --------------------------------------------------------------------------
///
/// \brief The CTrack class
///
class CTrack
{
public:
	//protected:
	//
	//	bool initGenderPredictor() {
	//		std::string gen_model_prefix = "D:/start/hunter/hunter_gender/model/gender/squeezenet-cs";
	//		int gen_epoch = 0;
	//		int gen_width = 128;
	//		int gen_height = 128;
	//		float gen_mean_r = 123.;
	//		float gen_mean_g = 117.;
	//		float gen_mean_b = 104.;
	//		int gen_device_type = 2;//if device_id
	//		int gen_device_id = 0;
	//		float gen_thresh = 0.5;
	//		g_gender = std::make_unique<gen::Gender>(gen_model_prefix, gen_epoch, gen_width, gen_height,
	//			gen_mean_r, gen_mean_g, gen_mean_b, gen_device_type, gen_device_id);
	//		return true;
	//	}
	//

public:
	///
	/// \brief CTrack
	/// \param pt
	/// \param region
	/// \param deltaTime
	/// \param accelNoiseMag
	/// \param trackID
	/// \param filterObjectSize
	/// \param externalTrackerForLost
	///
	//   

	CTrack(
		const Point_t& pt,
		const CRegion& region,
		TKalmanFilter::KalmanType kalmanType,
		track_t deltaTime,
		track_t accelNoiseMag,
		size_t trackID,
		bool filterObjectSize,
		bool externalTrackerForLost
	)
		:
		m_trackID(trackID),
		m_skippedFrames(0),
		m_lastRegion(region),
		m_predictionPoint(pt),
		m_filterObjectSize(filterObjectSize),
		m_externalTrackerForLost(externalTrackerForLost)
	{
		if (filterObjectSize)
		{
			m_kalman = new TKalmanFilter(kalmanType, region.m_rect, deltaTime, accelNoiseMag);
		}
		else
		{
			m_kalman = new TKalmanFilter(kalmanType, pt, deltaTime, accelNoiseMag);
		}
		m_trace.push_back(pt, pt);
	}

	///
	/// \brief CalcDist
	/// \param pt
	/// \return
	///
	track_t CalcDist(const Point_t& pt)
	{
		Point_t diff = m_predictionPoint - pt;
		return sqrtf(diff.x * diff.x + diff.y * diff.y);
	}

	///
	/// \brief CalcDist
	/// \param r
	/// \return
	///
	track_t CalcDist(const cv::Rect& r)
	{
		std::array<track_t, 4> diff;
		diff[0] = m_predictionPoint.x - m_lastRegion.m_rect.width / 2 - r.x;
		diff[1] = m_predictionPoint.y - m_lastRegion.m_rect.height / 2 - r.y;
		diff[2] = static_cast<track_t>(m_lastRegion.m_rect.width - r.width);
		diff[3] = static_cast<track_t>(m_lastRegion.m_rect.height - r.height);

		track_t dist = 0;
		for (size_t i = 0; i < diff.size(); ++i)
		{
			dist += diff[i] * diff[i];
		}
		return sqrtf(dist);
	}

	///
	/// \brief CalcOverlap
	/// \param r
	/// \return
	///
	track_t CalcDistJaccard(const cv::Rect& r)
	{
		cv::Rect rr(GetLastRect());

		track_t intArea = (r & rr).area();
		track_t unionArea = r.area() + rr.area() - intArea;

		return 1 - intArea / unionArea;
	}

	///
	/// \brief Update
	/// \param pt
	/// \param region
	/// \param dataCorrect
	/// \param max_trace_length
	/// \param prevFrame
	/// \param currFrame
	///
	void Update(
		const Point_t& pt,
		const CRegion& region,
		bool dataCorrect,
		size_t max_trace_length,
		cv::Mat prevFrame,
		cv::Mat currFrame
	)
	{
		if (m_filterObjectSize) // Kalman filter for object coordinates and size
		{
			RectUpdate(region, dataCorrect, prevFrame, currFrame);
		}
		else // Kalman filter only for object center
		{
			PointUpdate(pt, dataCorrect, currFrame.size());
		}

		if (dataCorrect)
		{
			m_lastRegion = region;
			m_trace.push_back(m_predictionPoint, pt);
		}
		else
		{
			m_trace.push_back(m_predictionPoint);
		}

		if (m_trace.size() > max_trace_length)
		{
			m_trace.pop_front(m_trace.size() - max_trace_length);
		}
	}

	///
	/// \brief IsRobust
	/// \param minTraceSize
	/// \param minRawRatio
	/// \param sizeRatio
	/// \return
	///
	bool IsRobust(int minTraceSize, float minRawRatio, cv::Size2f sizeRatio) const
	{
		bool res = m_trace.size() > static_cast<size_t>(minTraceSize);
		res &= m_trace.GetRawCount(m_trace.size() - 1) / static_cast<float>(m_trace.size()) > minRawRatio;
		if (sizeRatio.width + sizeRatio.height > 0)
		{
			float sr = m_lastRegion.m_rect.width / static_cast<float>(m_lastRegion.m_rect.height);
			if (sizeRatio.width > 0)
			{
				res &= (sr > sizeRatio.width);
			}
			if (sizeRatio.height > 0)
			{
				res &= (sr < sizeRatio.height);
			}
		}
		return res;
	}

	Trace m_trace;
	size_t m_trackID;
	size_t m_skippedFrames;
	CRegion m_lastRegion;
	Point_t m_averagePoint;   ///< Average point after LocalTracking
	cv::Rect m_boundidgRect;  ///< Bounding rect after LocalTracking

	cv::Rect GetLastRect() const
	{
		if (m_filterObjectSize)
		{
			return m_predictionRect;
		}
		else
		{
			return cv::Rect(
				static_cast<int>(m_predictionPoint.x - m_lastRegion.m_rect.width / 2),
				static_cast<int>(m_predictionPoint.y - m_lastRegion.m_rect.height / 2),
				m_lastRegion.m_rect.width,
				m_lastRegion.m_rect.height);
		}
	}

	InfoRect TrackIdstateUpdate(const Door_Params& door_params)
	{
		if (!flag_statues) {
			info.flag = 0;
			return info;
		}
		else {
			int doorLeft_x = door_params.door_left_x;
			int doorLeft_y = door_params.door_left_y;
			int doorRight_x = door_params.door_right_x;
			int doorRight_y = door_params.door_right_y;

			int current_status = 0;
			cv::Rect rr(GetLastRect());

			float arr[2] = { doorRight_x - doorLeft_x, doorRight_y - doorLeft_y };//694-336,639-604
			int rectHeight = floor(rr.height);
			cv::Point2i pre1(rr.x + rr.width / 2, rr.y + rectHeight);
			//cv::Point2i pre2(rr.x + rr.width / 2, rr.y);
			float predict_start[2] = { pre1.x - doorLeft_x, pre1.y - doorLeft_y };

			if (trackinDoor(door_params) && ((predict_start[0] * arr[1] - predict_start[1] * arr[0])> 0) && DistWithLine(door_params) > 20)
			{
				current_status = 1;//person_out
			}
			else if (trackinDoor(door_params) && ((predict_start[0] * arr[1] - predict_start[1] * arr[0])< 0) && DistWithLine(door_params) > 20) {
				current_status = -1;//person_in
			}
			else {
				current_status = 0;//person_in_line
			}

			info.rectstatus = m_status;

			info.rect = rr;
			if (m_status == 0) {
				m_status = current_status;
				info.flag = 0;
				return info;
			}
			else if ((m_status == 1) && (current_status == -1)) {
				flag_statues = false;
				m_status = current_status;
				info.flag = 1;
				return info;//get in
			}
			else if ((m_status == -1) && (current_status == 1)) {
				flag_statues = false;
				m_status = current_status;
				info.flag = -1;
				return info;//get out
			}
			info.flag = 0;
			return info;
		}
	}

	float DistWithLine(const Door_Params& door_params) {
		//
		cv::Rect rr(GetLastRect());

		int doorLeft_x = door_params.door_left_x;
		int doorLeft_y = door_params.door_left_y;
		int doorRight_x = door_params.door_right_x;
		int doorRight_y = door_params.door_right_y;
		cv::Point2i doorleft(doorLeft_x, doorLeft_y);
		cv::Point2i doorright(doorRight_x, doorRight_y);
		float Dist;
		cv::Point2i center(rr.x + rr.width / 2, rr.y + rr.height);
		//std::cout << center.x << "x" << center.y << "y" << std::endl;
		float up = abs((doorright.y - doorleft.y) * center.x + (doorleft.x - doorright.x)*center.y + doorleft.y * doorright.x - doorleft.x * doorright.y);
		float pow1 = pow(doorright.y - doorleft.y, 2);
		float pow2 = pow(doorright.x - doorleft.x, 2);
		float down = pow1 + pow2;
		double down2 = sqrt(down);
		//std::cout << "up = " << up << "down =" << down << "down2 =" << down2 << std::endl;
		return Dist = up / down2;
	}

	bool trackinDoor(const Door_Params& door_params) {
		int current_status = 0;
		cv::Rect rr(GetLastRect());
		int rectHeight = floor(rr.height);
		cv::Point2i pre1(rr.x + rr.width / 2, rr.y + rectHeight);
		if ((pre1.x>door_params.door_left_x) && (pre1.x<door_params.door_right_x)) {
			return true;
		}
		return false;
	}

	inline int argmax(const std::vector<float>& arr) {
		return std::distance(arr.begin(), std::max_element(arr.begin(), arr.end()));
	}

	void info_update(const InfoRect& info_new) {
		info.info_update(info_new,true);
	}
	InfoRect info_get() {
		return info;
	}
protected:
	InfoRect info;

private:
	int m_status = 0;
	//int frame_detect_time =0;
	bool flag_statues = true;// for each track judge it once, if one always out and in,count is no change 
	float m_distance = 0;
	bool * Flag = new bool[4];
	Point_t m_predictionPoint;
	cv::Rect m_predictionRect;
	TKalmanFilter* m_kalman;
	bool m_filterObjectSize;
	bool m_externalTrackerForLost;
	//KCFTracker tracker;
	std::unique_ptr<KCFTracker> m_tracker;

	//cv::Ptr<cv::Tracker> m_kcf_ptr;

	//std::unique_ptr<cv::Tracker> m_kcf_tracker;

	bool HOG = true;
	bool FIXEDWINDOW = false;
	bool MULTISCALE = true;
	bool SILENT = true;
	bool LAB = false;
	//std::unique_ptr<gen::Gender> g_gender;
	//virtual bool initGenderPredictor();
	///
	/// \brief RectUpdate
	/// \param region
	/// \param dataCorrect
	/// \param prevFrame
	/// \param currFrame
	///
	void RectUpdate(
		const CRegion& region,
		bool dataCorrect,
		cv::Mat prevFrame,
		cv::Mat currFrame
	)
	{
		m_kalman->GetRectPrediction();

		/*auto ratio = [](const cv::Rect& rect)->float
		{
			return rect.height / (float)rect.width;
		};*/

		bool recalcPrediction = true;

		if (m_externalTrackerForLost)
		{
			if (!dataCorrect && m_predictionRect.area()>0)
			{
				if (!m_tracker)
				{
					/*m_kcf_ptr = cv::TrackerKCF::create();
					m_kcf_ptr->init(prevFrame,m_predictionRect);*/
					m_tracker = std::make_unique<KCFTracker>(HOG, FIXEDWINDOW, MULTISCALE, LAB);
					////printf("init m_predictionRect %d %d %d %d\n", m_predictionRect.x, m_predictionRect.y, m_predictionRect.width, m_predictionRect.height);
					m_tracker->init(m_predictionRect, prevFrame);
				}

				//cv::Rect2d newRect;			
				cv::Rect newRect = m_tracker->update(currFrame);
				//printf("newRect %d %d %d %d\n", newRect.x, newRect.y, newRect.width, newRect.height);
				if (1)
				{
					m_predictionRect = m_kalman->Update(newRect, true);
					recalcPrediction = false;

					m_boundidgRect = cv::Rect();
					m_lastRegion.m_points.clear();
				}
			}
			else
			{
				if (m_tracker)
				{
					m_tracker.release();
				}
			}
		}

		if (recalcPrediction)
		{
			if (m_boundidgRect.area() > 0)
			{
				if (dataCorrect)
				{
					cv::Rect prect(
						(m_boundidgRect.x + region.m_rect.x) / 2,
						(m_boundidgRect.y + region.m_rect.y) / 2,
						(m_boundidgRect.width + region.m_rect.width) / 2,
						(m_boundidgRect.height + region.m_rect.height) / 2);

					m_predictionRect = m_kalman->Update(prect, dataCorrect);
				}
				else
				{
					cv::Rect prect(
						(m_boundidgRect.x + m_predictionRect.x) / 2,
						(m_boundidgRect.y + m_predictionRect.y) / 2,
						(m_boundidgRect.width + m_predictionRect.width) / 2,
						(m_boundidgRect.height + m_predictionRect.height) / 2);

					m_predictionRect = m_kalman->Update(prect, true);
				}
			}
			else
			{
				m_predictionRect = m_kalman->Update(region.m_rect, dataCorrect);
			}
		}
		if (m_predictionRect.width < 2)
		{
			m_predictionRect.width = 2;
		}
		if (m_predictionRect.x < 0)
		{
			m_predictionRect.x = 0;
		}
		else if (m_predictionRect.x + m_predictionRect.width > currFrame.cols - 1)
		{
			m_predictionRect.x = currFrame.cols - 1 - m_predictionRect.width;
		}
		if (m_predictionRect.height < 2)
		{
			m_predictionRect.height = 2;
		}
		if (m_predictionRect.y < 0)
		{
			m_predictionRect.y = 0;
		}
		else if (m_predictionRect.y + m_predictionRect.height > currFrame.rows - 1)
		{
			m_predictionRect.y = currFrame.rows - 1 - m_predictionRect.height;
		}

		m_predictionPoint = (m_predictionRect.tl() + m_predictionRect.br()) / 2;
	}

	///
	/// \brief PointUpdate
	/// \param pt
	/// \param dataCorrect
	///
	void PointUpdate(
		const Point_t& pt,
		bool dataCorrect,
		const cv::Size& frameSize
	)
	{
		m_kalman->GetPointPrediction();

		if (m_averagePoint.x + m_averagePoint.y > 0)
		{
			if (dataCorrect)
			{
				m_predictionPoint = m_kalman->Update((pt + m_averagePoint) / 2, dataCorrect);
			}
			else
			{
				m_predictionPoint = m_kalman->Update((m_predictionPoint + m_averagePoint) / 2, true);
			}
		}
		else
		{
			m_predictionPoint = m_kalman->Update(pt, dataCorrect);
		}

		if (m_predictionPoint.x < 0)
		{
			m_predictionPoint.x = 0;
		}
		else if (frameSize.width && m_predictionPoint.x > frameSize.width - 1)
		{
			m_predictionPoint.x = frameSize.width - 1;
		}
		if (m_predictionPoint.y < 0)
		{
			m_predictionPoint.y = 0;
		}
		else if (frameSize.width && m_predictionPoint.y > frameSize.height - 1)
		{
			m_predictionPoint.y = frameSize.height - 1;
		}
	}



};

typedef std::vector<std::unique_ptr<CTrack>> tracks_t;
